/*************************************************************
*
* M3D_LINE.CPP
*
* Lines / line segments mathematics (2D / 3D)
*************************************************************/
#include <stdafx.h>

//#include <ap_comm.h>
#include <m3d\m3d.h>

/*************************************************************
* m2dIsectLineLine()
* IN:  v1, v2 - first line
*      lB, lE - second line
* OUT: v      - point of intersection (valid if rc == TRUE)
* RET: TRUE   - lines intersect
*      FALSE  - otherwise
*
* Intersect two 2D lines
*************************************************************/
BOOL m2dIsectLineLine (m2dV *v1, m2dV *v2, m2dV *lB, m2dV *lE, m2dV *v)
{
   float d;
   float a, b, A, B; // to prevent optimization bugs in Release
   float s;

   a = v2->x - v1->x;
   b = v2->y - v1->y;

   A = lE->x - lB->x;
   B = lE->y - lB->y;

   d = A*b - B*a;

   if (d < M3D_EPSILON && d > -M3D_EPSILON)  {
      return FALSE;
   }

   s = ((lB->y - v1->y)*a - (lB->x - v1->x)*b) / d;

   v->x = lB->x + s*A;
   v->y = lB->y + s*B;
   return TRUE;
}

/*************************************************************
* m2dIsectLSegLSeg ()
*
* IN:  v1, v2 - coords of the endpoints of the first  line segment
*      lB, lE - coords of the endpoints of the second line segment
* OUT: v      - point of intersection (if exists)
* RET: TRUE   - line segments do intersect
*      FALSE  - line segments do not intersect
* 
* Intersect two line segments in 2D
*************************************************************/
BOOL m2dIsectLSegLSeg (m2dV *v1, m2dV *v2, m2dV *lB, m2dV *lE,
                       m2dV *v)
{
   float d;
   float a,b,A,B;
   float s;

   STRONG_ASSERT(v != NULL);

   a = v2->x - v1->x;
   b = v2->y - v1->y;
   A = lE->x - lB->x;
   B = lE->y - lB->y;

   d = A*b - B*a;

   if (d < M3D_EPSILON && d > -M3D_EPSILON )  {
      return FALSE;
   }

   s = ((lB->y - v1->y)*a - (lB->x - v1->x)*b) / d;
   // check that the point of intersection belongs to [lB,lE]
   if (s < -M3D_EPSILON || s > 1.f+M3D_EPSILON)  {
      return FALSE;
   }

   v->x = lB->x + s*A;
   v->y = lB->y + s*B;
   // check that the point of intersection belongs to [v1,v2]
   if (v->x < min(v1->x,v2->x) - 100.f*M3D_EPSILON || 
      v->x > max(v1->x,v2->x) + 100.f*M3D_EPSILON || 
      v->y < min(v1->y,v2->y) - 100.f*M3D_EPSILON || 
      v->y > max(v1->y,v2->y) + 100.f*M3D_EPSILON )  { 
         return FALSE;
   }

   return TRUE;
}

/*************************************************************
* m3xzIsectLSegLSeg ()
*
* IN:  v1, v2 - coords of the endpoints of the first  line segment
*      lB, lE - coords of the endpoints of the second line segment
* OUT: v      - point of intersection (if exists)
* RET: TRUE   - line segments do intersect
*      FALSE  - line segments do not intersect
* 
* Intersect two line segments in 3D-XZ
*************************************************************/
BOOL m3xzIsectLSegLSeg (m3dV *v1, m3dV *v2, m3dV *lB, m3dV *lE,
                        m3dV *v)
{
   m2dV _v1, _v2, _lB, _lE;
   m2dV _v;

   m3xzV3DToV2D(v1,&_v1);
   m3xzV3DToV2D(v2,&_v2);
   m3xzV3DToV2D(lB,&_lB);
   m3xzV3DToV2D(lE,&_lE);

   if (!m2dIsectLSegLSeg(&_v1,&_v2,&_lB,&_lE,&_v) )  {
      return FALSE;
   }
   if (v) {
      m3xzV2DToV3D(&_v,v);
   }

   return TRUE;
}

/*************************************************************
* m3xzIsectLineLine ()
*
* IN:  v1, v2 - two points on the first  line
*      lB, lE - two points on the second line
* OUT: v      - point of intersection (if exists)
* RET: TRUE   - lines do intersect
*      FALSE  - lines do not intersect
* 
* Intersect two lines in 3D-XZ
*************************************************************/
BOOL m3xzIsectLineLine (m3dV *v1, m3dV *v2, m3dV *lB, m3dV *lE,
                        m3dV *v)
{
   m2dV _v1, _v2, _lB, _lE;
   m2dV _v;

   m3xzV3DToV2D(v1,&_v1);
   m3xzV3DToV2D(v2,&_v2);
   m3xzV3DToV2D(lB,&_lB);
   m3xzV3DToV2D(lE,&_lE);

   if (!m2dIsectLineLine(&_v1,&_v2,&_lB,&_lE,&_v) )  {
      return FALSE;
   }

   if (v) {
      m3xzV2DToV3D(&_v,v);
   }

   return TRUE;
}

/*************************************************************
* BOOL m3dIsectLineLine_A()
*
* Intersection of two lines in space. 
* Ret: FALSE - lines do not intersect, v is not initialized
*      TRUE  - lines intersect,  v - intersection point  
*
* NOTE: See Bronshtein, p 223
*************************************************************/
BOOL m3dIsectLineLine_A (m3dV *v1, m3dV *v2, m3dV *lB, m3dV *lE, 
                         m3dV *v, float accuracy)
{
   m2dMATR  mat;
   m3dV     r1, r2;  // directing vectors of two lines
   float    t;

   // calculate directing vectors
   m3dSubtractVector(v2,v1,&r1);
   m3dSubtractVector(lE,lB,&r2);

   mat.ElementRC(0,0) = v1->x - lB->x;
   mat.ElementRC(0,1) = v1->y - lB->y;
   mat.ElementRC(0,2) = v1->z - lB->z;
   mat.ElementRC(1,0) = r1.x;
   mat.ElementRC(1,1) = r1.y;
   mat.ElementRC(1,2) = r1.z;
   mat.ElementRC(2,0) = r2.x;
   mat.ElementRC(2,1) = r2.y;
   mat.ElementRC(2,2) = r2.z;

   if (!m3dIsZero_A(mat.CalcDet(), accuracy) )  {
      return FALSE;
   }

   // tell AI if this ASSERT fails!!!
   ASSERT(!m3dIsZero(r1.x*r2.y - r1.y*r2.x));

   t = ((lB->x-v1->x)*r2.y - (lB->y-v1->y)*r1.x) / (r1.x*r2.y - r1.y*r2.x);

   v->x = v1->x + t*r1.x;
   v->y = v1->y + t*r1.y;
   v->z = v1->z + t*r1.z;

   return TRUE;
}

/*************************************************************
* BOOL m3dIsectLineLine()
*
* Intersection of two lines in space. 
* Ret: FALSE - lines do not intersect, v is not initialized
*      TRUE  - lines intersect,  v - intersection point  
*
*************************************************************/
BOOL m3dIsectLineLine (m3dV *l1, m3dV *l2, m3dV *v1, m3dV *v2, m3dV *v)
{
   m3dV n1 = *l1 - *l2; 
   m3dV n2 = *v1 - *v2;
   if (!_m3dNormalize(&n1) || !_m3dNormalize(&n2)) {
      return FALSE;
   }

   float a[3][2];
   a[0][0] = n1.x;
   a[0][1] = -n2.x;
   a[1][0] = n1.y;
   a[1][1] = -n2.y;
   a[2][0] = n1.z;
   a[2][1] = -n2.z;

   m3dV b = *v1 - *l1;

#define DET_A(i, j) a[i][0] * a[j][1] - a[i][1] * a[j][0]
#define CHECK_DET(i, j) !m3dIsZero_A(DET_A(i, j), 0.1f)
#define CALC_PARAM(i, j) if (!m3dSolveLinSys2D(a[i][0], a[i][1], a[j][0], a[j][1], b[i], b[j], &t1, &t2)) { return FALSE;}

   // solve square system of two components
   float t1 = -1.f, t2 = -1.f;

   // x & y
   if (CHECK_DET(0, 1)) {
     CALC_PARAM(0, 1);
   } else if (CHECK_DET(0, 2)) {
     CALC_PARAM(0, 2);
   } else if (CHECK_DET(1, 2)) {
      CALC_PARAM(1, 2);
   } else {
      return FALSE;
   }
   
   // get intersection
   m3dV li = *l1 + n1*t1;
   m3dV vi = *v1 + n2*t2;

   // check z component
   if (!m3dIsEqualPoint_A(&li, &vi, 100.f*M3D_EPSILON)) {
      return FALSE;
   }

   *v = li;

   return TRUE;
}

/*************************************************************
* BOOL m3dIsectLineLSeg()
*
* Intersection line and segment in space. 
* Ret: FALSE - lines do not intersect, v is not initialized
*      TRUE  - lines intersect,  v - intersection point  
*
*************************************************************/
BOOL m3dIsectLineLSeg (m3dV *l1, m3dV *l2, m3dV *lb, m3dV *le, m3dV *v)
{
   m3dV res;
   if (!m3dIsectLineLine(l1, l2, lb, le, &res)) {
      return FALSE;
   }

   float lx = le->x - lb->x;
   float ly = le->y - lb->y;
   float lz = le->z - lb->z;

   float minV = 1.5f;
   float maxV = -0.5f;

   if (m3dFAbs(lx) > M3D_EPSILON) {
      float t = (res.x - lb->x)/lx;
      minV = min(t, minV);
      maxV = max(t, maxV); 
   }

   if (m3dFAbs(ly) > M3D_EPSILON) {
      float t = (res.y - lb->y)/ly;
      minV = min(t, minV);
      maxV = max(t, maxV); 
   }

   if (m3dFAbs(lz) > M3D_EPSILON) {
      float t = (res.z - lb->z)/lz;
      minV = min(t, minV);
      maxV = max(t, maxV); 
   }

   if (!m3dIsEqual_A(minV, maxV, 0.1f)) {
      return FALSE;
   }

   if ((minV > 1.f + M3D_EPSILON) || 
      minV < -M3D_EPSILON) {
      return FALSE;
   }

   *v = res;

   return TRUE;
}

/************************************************************
* m3xzDistPointLSeg_2()
* m3xzDistPointLSeg()
*
* IN:  point    - point from which the distance is computed
*      lB, lE   - lsegment begin/end
* OUT: vClosest - closest point
* RET: distance from point to lsegment
*
* Calculate the (squared) distance between point and lseg in XZ
* If point is projected on lseg then the distance 
* is measured along the perpendicular; otherwise
* it is measured from the closest end-point
************************************************************/
BOOL m3xzDistPointLSeg_2 (m3dV *point, m3dV *lB, m3dV *lE, float distAlpha_2, float *dist_2, m3dV *vClosest)
{
   m3dV  lDir;
   float t, t1, d_2;
   m3dV  _vClosest, vC;

   if (!vClosest) {
      vClosest = &_vClosest;
   }

   m3dSubtractVector(lE,lB,&lDir);
   lDir.y = 0.f;
   t1 = m3dPow2(lDir.x) + m3dPow2(lDir.z);
   if (t1 < 10.f*M3D_EPSILON) {
      // lB-lE is degenerate
      d_2 = m3xzDist_2(lB, point);
      if (d_2 > distAlpha_2) {
         return FALSE;
      }
      *vClosest = *lB;
      *dist_2 = d_2;
      return TRUE;
   }
   t = ((point->x - lB->x)*lDir.x + (point->z - lB->z)*lDir.z)/t1;

   if (t < 0.f) {
      d_2 = m3xzDist_2(lB, point);
      if (d_2 > distAlpha_2) {
         return FALSE;
      }
      *vClosest = *lB;
      *dist_2 = d_2;
      return TRUE;
   }
   if (t > 1.f) {
      d_2 = m3xzDist_2(lE, point);
      if (d_2 > distAlpha_2) {
         return FALSE;
      }
      *vClosest = *lE;
      *dist_2 = d_2;
      return TRUE;
   }
   _m3dLineCombine(lB, &lDir, t, &vC);

   d_2 = m3xzDist_2(&vC, point);
   if (d_2 > distAlpha_2) {
      return FALSE;
   }
   *vClosest = vC;
   *dist_2 = d_2;
   return TRUE;
}

BOOL m3xzDistPointLSeg (m3dV *point, m3dV *lB, m3dV *lE, float distAlpha, float *dist, m3dV *vClosest)
{
   float d_2;
   if (!m3xzDistPointLSeg_2(point, lB, lE, distAlpha*distAlpha, &d_2, vClosest)) {
      return FALSE;
   }
   ASSERT(d_2 >= 0.f);
   *dist = m3dSqrt(d_2);
   ASSERT(*dist <= distAlpha + 0.001f);
   return TRUE;
}

/************************************************************
* m2dDistPointLSeg_2()
* m2dDistPointLSeg()
*
* IN:  point    - point from which the distance is computed
*      lB, lE   - lsegment begin/end
* OUT: vClosest - closest point
* RET: distance from point to lsegment
*
* Calculate the (squared) distance between point and lseg
* If point is projected on lseg then the distance 
* is measured along the perpendicular; otherwise
* it is measured from the closest end-point
************************************************************/
float m2dDistPointLSeg_2(m2dV *point, m2dV *lB, m2dV *lE, m2dV *vClosest, float *tMin)
{
   m2dV  lDir;
   m2dV  tmpClosest;
   float t;

   m2dSubtractVector(lE,lB,&lDir);
   t = ((point->x - lB->x)*lDir.x + (point->y - lB->y)*lDir.y)/(m3dPow2(lDir.x) + m3dPow2(lDir.y));

   if (t < 0.f) {
      if (vClosest) {
         *vClosest = *lB;
      }
      if (tMin) {
         *tMin = 0.f;
      }
      return m2dDist_2(lB,point);
   }
   if (t > 1.f) {
      if (vClosest) {
         *vClosest = *lE;
      }
      if (tMin) {
         *tMin = 1.f;
      }
      return m2dDist_2(lE,point);
   }

   m2dScaleVector(&lDir,t,&lDir);
   m2dAddVector(lB,&lDir,&tmpClosest);
   if (vClosest) {
      *vClosest = tmpClosest;
   }
   if (tMin) {
      *tMin = t;
   }
   return m2dDist_2(&tmpClosest,point);
}

float m2dDistPointLSeg (m2dV *point, m2dV *lB, m2dV *lE, m2dV *vClosest, float *tMin)
{
   return m3dSqrt(m2dDistPointLSeg_2(point, lB, lE, vClosest, tMin));
}
/**************************************************************************
* float m2dDistPointLine()
*
* IN :A,B   - points on line
*     Point - point
*
* RET: absolete distance from point Point to line AB
***************************************************************************/
float m2dDistPointLine(const m2dV *pPoint, const m2dV *pA, const m2dV *pB)
{
   float dist;
   m2dV  p, a;

   m2dSubtractVector(pB, pA, &a);
   m2dSubtractVector(pPoint, pA, &p);

   m2dNormalize(&a);

   _m2dScaleVector(&a, m2dDotProduct(&a, &p));

   dist = m2dDist(&p, &a);

   ASSERT((dist < (m2dDist(pPoint, pA) + 100.f*M3D_EPSILON)) && (dist < (m2dDist(pPoint, pB) + 100.f*M3D_EPSILON)));
   return dist;
} 
/**************************************************************************
* m2dDistPointLine_S ()
*
* IN :A,B   - points on line
*     Point - point
*
* RET: distance from point Point to line AB with SIGN
***************************************************************************/
float m2dDistPointLine_S (m2dV *pPoint, m2dV *pA, m2dV *pB)
{
   float a, b, c;
   float m;

   a = pB->y - pA->y;
   b = pA->x - pB->x;
   c = (pA->y*pB->x - pB->y*pA->x);
   m = (float)m3dSqrt(a*a+b*b);

   return (a * pPoint->x + b * pPoint->y + c) / m;
} 

/*************************************************************
* m3dDistPointLine ()
*
* Calculate the distance between point and a line
* NOTE: See Bronstein, p. 223
*************************************************************/
float m3dDistPointLine (const m3dV *point, const m3dV *lB, const m3dV *lE)
{
   m3dV  dir;
   float a, b, c;
   float l, m, n;
   float x1, y1, z1;

   m3dSubtractVector(lE,lB,&dir);

   a  = point->x; b = point->y;  c = point->z;
   l  = dir.x;    m = dir.y;     n = dir.z;
   x1 = lB->x;    y1 = lB->y;    z1 = lB->z;
   
   return (float)m3dSqrt((m3dPow2((a-x1)*m-(b-y1)*l) + m3dPow2((b-y1)*n-(c-z1)*m) + m3dPow2((c-z1)*l-(a-x1)*n)))/m3dLengthVector(&dir);
}

/*************************************************************
* m3dDistPointLine()
*
* Calculate the distance between point and a line
* REF: See Bronstein, p. 223
*************************************************************/
float m3dDistPointLine (const m3dV *point, const m3dV *lB, const m3dV *lE, m3dV *vClosest)
{
   float d, s;
   m3dV  vPointToB, dir;
   
   if (m3dIsZero(m3dDist(lE,lB))) {
      if (vClosest) {
         *vClosest = *lB;
      }
      return m3dDist(point,lB);
   }

   d = m3dDistPointLine(point,lB,lE);
   if (!vClosest) {
      return d;
   }

   // l = m3dSqrt(m3dFAbs( - d*d + m3dDist_2(point,lB)));

   m3dSubtractVector(lE,lB,&dir);
   m3dNormalize(&dir);

   m3dSubtractVector(lB, point, &vPointToB);

   s = m3dDotProduct(&vPointToB, &dir);
   _m3dLineCombine(lB, &dir, -s, vClosest);
   return d;
}

/******************************************************************************
* m3dDistPointLSeg()
*
* IN:  point  - point of interest
*      lB, lE - specify the line segment
*      distAlpha - alpha cut distance
*
* OUT: dist     - distance from point to line segment
*      vClosest - closest point on line segment
*      tMin     - parameter 't' along the line
* RET: FALSE if segment is far away from point than distAlpha
*
* Calculate the distance between point and a line segment
* If 'point' is projected onto the lseg, the distance is
* measured along the perpendicular; otherwise it is
* measured to the closest end-point of the LSeg
******************************************************************************/
BOOL m3dDistPointLSeg (const m3dV *point, m3dV *lB, m3dV *lE, float distAlpha, float *dist, m3dV *vClosest, float *tMin)
{
   m3dV     n;               // direction of segment
   m3dV     p;               // vector from begin of segment to point
   float    a, b;            // coefficients of quadratic form
   float    t;               // t that realizes minimum
   float    minDist;         // distance between segments
   m3dV     closest;         // closest points

   ASSERT(point != NULL);
   ASSERT(lB != NULL && lE != NULL);

   // calculate direction of segment
   m3dSubtractVector(lE, lB, &n);
   if (m3dIsVZero(&n)) {
      minDist = m3dDist(point, lE);
      if (minDist < distAlpha) {
         if (dist != NULL) {
            *dist = minDist;
         }
         if (vClosest != NULL) {
            *vClosest = *lB;
         }
         if (tMin != NULL) {
            *tMin = 0.f;
         }
         return TRUE;
      }
      return FALSE;
   }

   m3dSubtractVector(point, lB, &p);

   // calculate quadratic form coefficients
   a = m3dLengthVector_2(&n);
   b = -2.f * m3dDotProduct(&p, &n);

   // find t that realizes minimum of quadratic form on segment
   t = -b / (2.f * a);
   t = _m3dClamp(0.f, 1.f, t);

   _m3dLineCombine(lB, &n, t, &closest);
   minDist = m3dDist(&closest, point);

   // t cutting off
   if (minDist >= distAlpha) {
      return FALSE;
   }

   // store results
   if (dist != NULL) {
      *dist = minDist;
   }
   if (vClosest != NULL) {
      *vClosest = closest;
   }
   if (tMin != NULL) {
      *tMin = t;
   }
   return TRUE;
}

/******************************************************************************
* m3dDistLSegLSeg()
*
* IN:  lB1, lE1 - first  line segment
*      lB2, lE2 - second line segment
*      distMax  - alpha cutting off
* OUT: dist     - distance between line segments
*      vClos1   - closest point on the first line segment
*      vClos2   - closest point on the second line segment
* RET: TRUE     - distance between line segments is not more than distMax
*      FALSE    - the distance between line segments is more than distMax
*
* Compute the distance between two line segments in 3D
******************************************************************************/
BOOL m3dDistLSegLSeg (m3dV *lB1, m3dV *lE1, m3dV *lB2, m3dV *lE2, float distMax, float *dist, m3dV *vClos1, m3dV *vClos2)
{
   m3dV     n1;                    // direction of first segment (not normalized)
   m3dV     n2;                    // direction of second segment (not normalized)
   m3dV     p;                     // vector from begin of first segement to begin of second
   float    a2, b2, c, a, b, d;    // coefficients of quadratic form
   float    t, s;                  // t and s that are realize minimum and value of minimum
   float    t1 = 0, s1 = 0, min1 = 0;  // intermediate t and s on border of square
   float    t2 = 0, s2 = 0, min2 = 0;  // intermediate t and s on border of square
   BOOL     check1, check2;        // 
   float    minDist;               // distance between segments
   m3dV     closest1, closest2;    // closest points on segments

   ASSERT(lB1 != NULL && lE1 != NULL);
   ASSERT(lB2 != NULL && lE2 != NULL);

   // calculate direction of first segment
   m3dSubtractVector(lE1, lB1, &n1);
   if (m3dIsVZero(&n1)) {
      if (m3dDistPointLSeg(lB1, lB2, lE2, distMax, dist, vClos2, NULL)) {
         if (vClos1 != NULL) {
            *vClos1 = *lB1;
         }
         return TRUE;
      }
      return FALSE;
   }

   // calculate direction of second segment
   m3dSubtractVector(lE2, lB2, &n2);
   if (m3dIsVZero(&n2)) {
      if (m3dDistPointLSeg(lB2, lB1, lE1, distMax, dist, vClos1, NULL)) {
         if (vClos2 != NULL) {
            *vClos2 = *lB2;
         }
         return TRUE;
      }
      return FALSE;
   }

   m3dSubtractVector(lB1, lB2, &p);

   // calculate quadratic form coefficients
   a2 = m3dLengthVector_2(&n1);
   b2 = m3dLengthVector_2(&n2);
   c = -2.f * m3dDotProduct(&n1, &n2);
   a = 2.f * m3dDotProduct(&p, &n1);
   b = -2.f * m3dDotProduct(&p, &n2);
   d = m3dLengthVector_2(&p);

   // find minimum of quadratic form
   if (!m3dSolveLinSys2D(2.f * a2, c, c, 2.f * b2, -a, -b, &t, &s)) {
      t = a / (2.f * a2);
      s = 0.f;
   }

   // find candidate points on square border
   check1 = TRUE;
   if (t < 0.f) {
      t1 = 0.f;
      s1 = -b / (2.f * b2);
      s1 = _m3dClamp(0.f, 1.f, s1);
      min1 = (b2 * s1 + b) * s1 + d;
   } else if (1.f < t) {
      t1 = 1.f;
      s1 = -(c + b) / (2.f * b2);
      s1 = _m3dClamp(0.f, 1.f, s1);
      min1 = (b2 * s1 + c + b) * s1 + a2 + a + d;
   } else {
      check1 = FALSE;
   }
   check2 = TRUE;
   if (s < 0.f) {
      s2 = 0.f;
      t2 = -a / (2.f * a2);
      t2 = _m3dClamp(0.f, 1.f, t2);
      min2 = (a2 * t2 + a) * t2 + d;
   } else if (1.f < s) {
      s2 = 1.f;
      t2 = -(c + a) / (2.f * a2);
      t2 = _m3dClamp(0.f, 1.f, t2);
      min2 = (a2 * t2 + c + a) * t2 + b2 + b + d;
   } else {
      check2 = FALSE;
   }

   // find minimum of quadratic form on square [0, 1] * [0, 1]
   if (check1) {
      if (check2) {
         if (min1 < min2) {
            s = s1;
            t = t1;
         } else {
            s = s2;
            t = t2;
         }
      } else {
         s = s1;
         t = t1;
      }
   } else {
      if (check2) {
         s = s2;
         t = t2;
      }
   }

   ASSERT(0.f <= t && t <= 1.f);
   ASSERT(0.f <= s && s <= 1.f);

   _m3dLineCombine(lB1, &n1, t, &closest1);
   _m3dLineCombine(lB2, &n2, s, &closest2);
   minDist = m3dDist(&closest1, &closest2);

   // alpha cutting off
   if (minDist >= distMax) {
      return FALSE;
   }

   // store results
   if (dist != NULL) {
      *dist = minDist;
   }
   if (vClos1 != NULL) {
      *vClos1 = closest1;
   }
   if (vClos2 != NULL) {
      *vClos2 = closest2;
   }
   return TRUE;
}

/*************************************************************
* m3dDistPointLSeg()
*
* IN:  point  - point of interest
*      lB, lE - specify the line segment
* OUT: t      - parameter 't' along the line
*
* Calculate the distance between point and a line segment
* If 'point' is projected onto the lseg, the distance is
* measured along the perpendicular; otherwise it is
* measured to the closest end-point of the LSeg
*************************************************************/
float m3dDistPointLSeg (const m3dV *point, m3dV *lB, m3dV *lE, m3dV *vClosest)
{
   float dMin;
   BOOL  rc;

   rc = m3dDistPointLSeg(point, lB, lE, M3D_INFINITY, &dMin, vClosest);
   ASSERT(rc);
   return dMin;
}

/*************************************************************************
* m3dDistLineLine()
*
* Calculate distance between lines
* IN:  lB1, lE1 - first  line begin/end point (must be different)
*      lB2, lE2 - second line begin/end point (must be different)
* RET: distance between lines given by line segments
* REF: Bronshtein, page 223
*************************************************************************/
float m3dDistLineLine (m3dV *lB1, m3dV *lE1, m3dV *lB2, m3dV *lE2)
{
   float det_lm, det_mn, det_nl, det;
   float dist_2, factor_2;
   m3dV  vTmp, vTmp_cross;

   m3dV  dir1, dir2;

   m3dSubtractVector(lE1, lB1, &dir1);
   m3dSubtractVector(lE2, lB2, &dir2);

   ASSERT(!m3dIsZero(m3dLengthVector(&dir1)));
   ASSERT(!m3dIsZero(m3dLengthVector(&dir2)));

   det_lm = dir1.x * dir2.y - dir1.y * dir2.x;
   det_mn = dir1.y * dir2.z - dir1.z * dir2.y;
   det_nl = dir1.z * dir2.x - dir1.x * dir2.z;

   factor_2 = m3dPow2(det_lm) + m3dPow2(det_mn) + m3dPow2(det_nl);
   if (m3dIsZero_A(factor_2, 0.0001f)) {
      // lines are parallel
      m3dSubtractVector(lB2, lB1, &vTmp);
      m3dNormalize(&dir1);
      m3dCrossProduct(&vTmp, &dir1, &vTmp_cross);
      dist_2 = m3dLengthVector_2(&vTmp_cross);
   } else {
      // lines are not parallel
      det = (lB1->x - lB2->x) * det_mn + 
            (lB1->y - lB2->y) * det_nl +
            (lB1->z - lB2->z) * det_lm;
   
      dist_2 = m3dPow2(det)/factor_2;
   }

   return m3dSqrt(dist_2);
}

/*********************************************************************
* m3dIsIsectYLinePlane()
*
* IN : pLineOrg     - line origin
*      pPlaneOgr    - point in the plane
*      pPlaneNormal - plane normal
* OUT: pIsect - intersection point of plane with vertical line passing through 'pLineOrg'
* RET: FALSE if intersection not found (line is parallel to plane)
*********************************************************************/
BOOL m3dIsIsectYLinePlane (m3dV *pLineOrg, m3dV *pPlaneOrg, m3dV *pPlaneNormal, m3dV *pIsect)
{
   m3dV  vLineToPlane;
   float alpha;

   if (m3dIsZero_A(pPlaneNormal->y, 100*M3D_EPSILON)) {
      return FALSE;
   }
   if (pIsect) {
      m3dSubtractVector(pPlaneOrg, pLineOrg, &vLineToPlane);
      alpha = m3dDotProduct(&vLineToPlane, pPlaneNormal)/pPlaneNormal->y;
   
      _m3dLineCombine(pLineOrg, &m3dVUnitY, alpha, pIsect);

#ifdef _DEBUG
      /*
      m3dV  vTmp;
      float product;
      m3dSubtractVector(pPlaneOrg, pIsect, &vTmp);
      // how about to normalize vTmp?????
      product = m3dDotProduct(&vTmp, pPlaneNormal);
      ASSERT(m3dIsZero_A(product,100*M3D_EPSILON));
      */
#endif
   }
   return TRUE;
}

//
// End-of-file M3D_LINE.CPP
//
